copp\copp\copp2/
interpolation.rs

1//! Interpolation and profile-conversion utilities for second-order path parameterization.
2//!
3//! # Method identity
4//! This module serves both:
5//! - **Time-Optimal Path Parameterization (TOPP2)** workflows,
6//! - **Convex-Objective Path Parameterization (COPP2)** workflows.
7//!
8//! # Scope
9//! This module provides deterministic conversions among:
10//! - path-parameter profile `a(s) = \dot{s}^2`,
11//! - derivative-like profile `b(s) = \frac{1}{2}\frac{da}{ds}` on segments,
12//! - time mapping `t(s)` and inverse sampling `s(t)`.
13//!
14//! # Conventions
15//! - Path grid uses station samples `s[0..=n]`.
16//! - State profile `a` is node-based (`a.len() == s.len()`).
17//! - Profile `b` is segment-based (`b.len() == s.len() - 1`).
18
19use crate::copp::InterpolationMode;
20use crate::diag::{
21    CoppError, check_input_len_at_least, check_input_len_equal, check_input_not_empty,
22    check_input_not_nan_infinite, check_input_slice_non_negative,
23    check_input_slice_not_nan_infinite, check_input_strictly_increasing,
24};
25use itertools::izip;
26
27/// Compute segment profile `b` from node profile `a`.
28///
29/// # Definition
30/// For each segment `[s_k, s_{k+1}]`, this function computes:
31/// $b_k = \frac{1}{2}\frac{a_{k+1}-a_k}{s_{k+1}-s_k}$.
32///
33/// # Input contract
34/// - valid when `s.len() >= 2` and `a.len() == s.len()`;
35/// - `s` must be finite and strictly increasing;
36/// - `a` must contain only finite values.
37///
38/// # Returns
39/// Returns `b` with `b.len() == s.len() - 1`.
40///
41/// # Errors
42/// Returns [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput) when dimensions, monotonicity, or numeric
43/// finiteness requirements are violated.
44///
45/// # Contract
46/// - Output ordering is consistent with segment ordering on `s.windows(2)`.
47/// - No allocation beyond returned vector and iterator temporaries.
48pub fn a_to_b_topp2(s: &[f64], a: &[f64]) -> Result<Vec<f64>, CoppError> {
49    check_topp2_sa("a_to_b_topp2", s, a)?;
50    Ok(s.windows(2)
51        .zip(a.windows(2))
52        .map(|(s_pair, a_pair)| 0.5 * (a_pair[1] - a_pair[0]) / (s_pair[1] - s_pair[0]))
53        .collect::<Vec<f64>>())
54}
55
56/// Compute cumulative time profile `t(s)` from `a(s)`.
57///
58/// # Semantics
59/// - `t_s[i]` is the time at station `s[i]`.
60/// - initial condition is `t_s[0] = t0`.
61/// - returns `(t_final, t_s)` where `t_final == *t_s.last().unwrap()`.
62///
63/// # Input contract
64/// - valid when `s.len() >= 2` and `a.len() == s.len()`;
65/// - `s`, `a`, and `t0` must contain only finite values;
66/// - `s` must be strictly increasing;
67/// - each interval must have finite positive speed denominator.
68///
69/// # Returns
70/// Returns `(t_final, t_s)` with `t_s.len() == s.len()` on valid input.
71///
72/// # Errors
73/// Returns [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput) when dimensions, monotonicity, positivity,
74/// or numeric finiteness requirements are violated.
75///
76/// # Contract
77/// - `t_s` is monotonically increasing when `a` is nonnegative and `s` is increasing.
78/// - `t_s[0] == t0` always holds on valid input.
79pub fn s_to_t_topp2(s: &[f64], a: &[f64], t0: f64) -> Result<(f64, Vec<f64>), CoppError> {
80    check_topp2_sa("s_to_t_topp2", s, a)?;
81    check_input_not_nan_infinite("s_to_t_topp2", "t0", t0)?;
82    check_topp2_time_denominator("s_to_t_topp2", a)?;
83    // Map s to t
84    let mut t_s = Vec::<f64>::with_capacity(s.len()); // t_s[i] = t(s[i]), begin from t0
85    let mut t_prev = t0;
86    t_s.push(t_prev);
87    for (s_pair, a_pair) in s.windows(2).zip(a.windows(2)) {
88        t_prev += 2.0 * (s_pair[1] - s_pair[0]) / (a_pair[0].sqrt() + a_pair[1].sqrt());
89        t_s.push(t_prev);
90    }
91    if !t_prev.is_finite() || t_s.iter().any(|value| !value.is_finite()) {
92        return Err(CoppError::InvalidInput(
93            "s_to_t_topp2".into(),
94            "computed time profile contains NaN or infinity".into(),
95        ));
96    }
97    Ok((t_prev, t_s))
98}
99
100/// Interpolate inverse mapping `s(t)` from `a(s)` and sampled `t(s)`.
101///
102/// # Modes
103/// - [`UniformTimeGrid`](crate::InterpolationMode::UniformTimeGrid)`(t0, dt, include_final)`: generate uniform time samples;
104/// - `NonUniformTimeGrid(t_sample)`: use caller-provided increasing samples.
105///
106/// # Input contract
107/// - requires `s.len() >= 2`, `a.len() == s.len()`, `t_s.len() == s.len()`;
108/// - requires `t_s` strictly increasing.
109/// - all profile and time-grid values must be finite.
110///
111/// # Output semantics
112/// - output length matches requested sample count in each mode;
113/// - for out-of-range time samples, output entries are `NaN`.
114///
115/// # Returns
116/// Returns sampled `s(t)` values according to `mode`.
117///
118/// # Errors
119/// Returns [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput) when dimensions, monotonicity, positivity,
120/// or numeric finiteness requirements are violated.
121///
122/// # Contract
123/// - preserves requested sample order;
124/// - malformed input is reported as [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput).
125pub fn t_to_s_topp2(
126    s: &[f64],
127    a: &[f64],
128    t_s: &[f64],
129    mode: InterpolationMode<'_>,
130) -> Result<Vec<f64>, CoppError> {
131    check_topp2_sa("t_to_s_topp2", s, a)?;
132    check_topp2_time_denominator("t_to_s_topp2", a)?;
133    check_input_len_equal(
134        "t_to_s_topp2",
135        "`t_s.len()`",
136        t_s.len(),
137        "`s.len()`",
138        s.len(),
139    )?;
140    check_input_slice_not_nan_infinite("t_to_s_topp2", "t_s", t_s)?;
141    check_input_strictly_increasing("t_to_s_topp2", "t_s", t_s)?;
142    match mode {
143        InterpolationMode::UniformTimeGrid(t0, dt, include_final) => {
144            check_input_not_nan_infinite("t_to_s_topp2", "t0", t0)?;
145            check_input_not_nan_infinite("t_to_s_topp2", "dt", dt)?;
146            if dt <= 0.0 {
147                return Err(CoppError::InvalidInput(
148                    "t_to_s_topp2".into(),
149                    format!("`dt` = {dt} must be positive"),
150                ));
151            }
152            // num_t * dt + t0 <= t_final
153            let num_t = ((t_s.last().unwrap() - t0) / dt).floor() as usize;
154            let mut s_t =
155                t_to_s_topp2_core(s, a, t_s, (0..num_t).map(|i| t0 + i as f64 * dt), num_t);
156            if include_final {
157                let flag = if s_t.is_empty() {
158                    t0 <= *t_s.last().unwrap()
159                } else {
160                    *s_t.last().unwrap() < *s.last().unwrap()
161                };
162                if flag {
163                    s_t.push(*s.last().unwrap());
164                }
165            }
166            Ok(s_t)
167        }
168        InterpolationMode::NonUniformTimeGrid(t_sample) => {
169            check_input_not_empty("t_to_s_topp2", "`t_sample`", t_sample.len())?;
170            check_input_slice_not_nan_infinite("t_to_s_topp2", "t_sample", t_sample)?;
171            check_input_strictly_increasing("t_to_s_topp2", "t_sample", t_sample)?;
172            Ok(t_to_s_topp2_core(
173                s,
174                a,
175                t_s,
176                t_sample.iter().cloned(),
177                t_sample.len(),
178            ))
179        }
180    }
181}
182
183/// Check the shared TOPP2 profile shape and station-ordering contract.
184///
185/// The second-order interpolation routines all require a node-based `a(s)`
186/// profile sampled on the same strictly increasing station grid `s`.
187fn check_topp2_sa(function_name: &str, s: &[f64], a: &[f64]) -> Result<(), CoppError> {
188    check_input_len_at_least(function_name, "`s.len()`", s.len(), 2)?;
189    check_input_len_equal(function_name, "`a.len()`", a.len(), "`s.len()`", s.len())?;
190    check_input_slice_not_nan_infinite(function_name, "s", s)?;
191    check_input_slice_not_nan_infinite(function_name, "a", a)?;
192    check_input_strictly_increasing(function_name, "s", s)
193}
194
195/// Check the TOPP2 time-integration denominator.
196///
197/// The mapping from `s` to `t` divides by
198/// `sqrt(a[i]) + sqrt(a[i + 1])`; this helper rejects negative `a` values and
199/// zero-speed intervals before the integration loop.
200fn check_topp2_time_denominator(function_name: &str, a: &[f64]) -> Result<(), CoppError> {
201    check_input_slice_non_negative(function_name, "a", a)?;
202    if let Some((index, _pair)) = a.windows(2).enumerate().find(|(_, pair)| {
203        let denominator = pair[0].sqrt() + pair[1].sqrt();
204        !denominator.is_finite() || denominator <= 0.0
205    }) {
206        return Err(CoppError::InvalidInput(
207            function_name.into(),
208            format!(
209                "`sqrt(a[{index}]) + sqrt(a[{}])` must be finite and positive",
210                index + 1
211            ),
212        ));
213    }
214    Ok(())
215}
216
217/// Core inverse interpolation kernel for [`t_to_s_topp2`](crate::solver::topp2_ra::t_to_s_topp2).
218///
219/// It consumes increasing `t_sample` values and emits corresponding `s(t)` by
220/// segment-wise inversion with quadratic-in-`a` local model.
221fn t_to_s_topp2_core(
222    s: &[f64],
223    a: &[f64],
224    t_s: &[f64],
225    mut t_sample: impl Iterator<Item = f64>,
226    len_t_sample: usize,
227) -> Vec<f64> {
228    let &t_start = t_s.first().unwrap();
229    // Map t to s
230    let mut s_t = Vec::<f64>::with_capacity(len_t_sample + 1); // s_t[i] = s(t[i])
231    let Some(mut t_curr) = t_sample.next() else {
232        return vec![];
233    };
234    while t_curr < t_start {
235        s_t.push(f64::NAN);
236        let Some(t) = t_sample.next() else {
237            return s_t;
238        };
239        t_curr = t;
240    }
241
242    for (s_pair, a_pair, t_pair) in izip!(s.windows(2), a.windows(2), t_s.windows(2)) {
243        while t_curr <= t_pair[1] {
244            s_t.push(
245                s_pair[0]
246                    + inverse_2order(
247                        a_pair[0],
248                        (a_pair[1] - a_pair[0]) / (s_pair[1] - s_pair[0]),
249                        0.0,
250                        t_curr - t_pair[0],
251                    ),
252            );
253            let Some(t) = t_sample.next() else {
254                return s_t;
255            };
256            t_curr = t;
257        }
258    }
259
260    s_t.push(f64::NAN);
261    while t_sample.next().is_some() {
262        s_t.push(f64::NAN);
263    }
264    s_t
265}
266
267/// Solve `x_right` from the integral equation
268/// $dt = \int_{x_{left}}^{x_{right}} \frac{dx}{\sqrt{c_0 + c_1 x}}$.
269#[inline]
270fn inverse_2order(c0: f64, c1: f64, x_left: f64, dt: f64) -> f64 {
271    if dt == 0.0 {
272        x_left
273    } else if c1.abs() > f64::EPSILON {
274        (((c0 + c1 * x_left).sqrt() + 0.5 * c1 * dt).powi(2) - c0) / c1
275    } else if c0.abs() > f64::EPSILON {
276        x_left + c0.sqrt() * dt
277    } else {
278        f64::INFINITY
279    }
280}
copp\copp\copp2/interpolation.rs

copp\copp\copp2/
interpolation.rs
